#include <ros/ros.h>
#include <chrono> // 用于高精度计时
#include <iostream>
#include <string>
#include <vector> // 存储反序列化时间
#include <numeric> // 用于 std::accumulate
#include <cmath>   // 用于 std::sqrt

// 包含你的 ROS 原生消息头文件
// 替换为你的实际 ROS 包名和消息类型
#include <myproject/Person.h> // 例如：myproject/msg/Person.msg 生成的头文件

// 用于存储时间的全局向量和互斥锁 (如果需要多线程访问)
std::vector<long long> deserialization_times_ns;
// std::mutex g_mutex; // 默认 ros::spin() 单线程处理回调，此行通常不需要，但多线程时是必须的

// 定义收集消息数量阈值，达到后自动关闭节点并打印结果
// 设置为 0 表示无限收集，直到手动 Ctrl+C 停止
#define DESERIALIZATION_COUNT_THRESHOLD 1000 // 与 Protobuf 测试保持一致

// 消息回调函数
void messageCallback(const myproject::Person::ConstPtr& received_msg) {
    // --- 开始计时反序列化 ---
    auto start = std::chrono::high_resolution_clock::now();

    // 当 ROS 调用你的回调函数时，消息已经完全反序列化到 received_msg 对象中。
    // 为了确保测量的是完整的反序列化耗时，我们访问消息的一些字段，
    // 强制数据被“使用”或“实例化”。
    std::string name_accessed = received_msg->name;
    std::string address_accessed = received_msg->address;
    std::string num_accessed = received_msg->num;
    // 如果消息包含数组，也访问它们以确保完整性：
    // for (const auto& val : received_msg->int_array) { /* do nothing */ }
    // for (const auto& val : received_msg->float_array) { /* do nothing */ }


    auto end = std::chrono::high_resolution_clock::now();
    // --- 结束计时反序列化 ---

    std::chrono::nanoseconds duration = std::chrono::duration_cast<std::chrono::nanoseconds>(end - start);

    // std::lock_guard<std::mutex> lock(g_mutex); // 如果使用多线程，需要加锁保护
    deserialization_times_ns.push_back(duration.count());

    // 可选：实时打印每条消息的耗时
    // std::cout << "反序列化时间 (回调函数开始到访问数据): " << duration.count() << " 纳秒" << std::endl;

    // 检查是否已收集到足够样本，然后关闭 ROS
    if (DESERIALIZATION_COUNT_THRESHOLD > 0 && deserialization_times_ns.size() >= DESERIALIZATION_COUNT_THRESHOLD) {
        ROS_INFO("Collected %d deserialization samples. Shutting down to print results.", DESERIALIZATION_COUNT_THRESHOLD);
        ros::shutdown(); // 触发 ros::spin() 退出
    }
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "ros_native_subscriber_node");
    ros::console::set_logger_level(ROSCONSOLE_DEFAULT_NAME,
                                   ros::console::levels::Debug);
    ros::NodeHandle nh;

    // 订阅话题，使用你的 ROS 原生消息类型
    ros::Subscriber sub = nh.subscribe("/my_ros_native_topic", 1000, messageCallback); // 队列大小 1000

    ROS_INFO("ROS 原生订阅节点已启动。等待消息...");
    if (DESERIALIZATION_COUNT_THRESHOLD > 0) {
        ROS_INFO("Will collect %d deserialization samples then print average.", DESERIALIZATION_COUNT_THRESHOLD);
    } else {
        ROS_INFO("将无限收集反序列化样本。请按 Ctrl+C 停止并打印平均值。");
    }

    ros::spin(); // 保持节点运行，直到 ros::shutdown() 被调用或 Ctrl+C

    // --- ros::spin() 退出后进行统计计算 ---
    if (!deserialization_times_ns.empty()) {
        long long total_duration_ns = 0;
        for (long long time_ns : deserialization_times_ns) {
            total_duration_ns += time_ns;
        }

        double average_duration_ns = static_cast<double>(total_duration_ns) / deserialization_times_ns.size();
        double average_duration_us = average_duration_ns / 1000.0;
        double average_duration_ms = average_duration_us / 1000.0;

        std::cout << "\n--- ROS 原生消息反序列化性能测试结果 ---" << std::endl;
        std::cout << "总反序列化次数: " << deserialization_times_ns.size() << std::endl;
        std::cout << "总耗时: " << total_duration_ns / 1e9 << " 秒 (" << total_duration_ns << " 纳秒)" << std::endl;
        std::cout << "平均每次反序列化耗时: " << average_duration_ns << " 纳秒" << std::endl;
        std::cout << "平均每次反序列化耗时: " << average_duration_us << " 微秒" << std::endl;
        std::cout << "平均每次反序列化耗时: " << average_duration_ms << " 毫秒" << std::endl;

        if (deserialization_times_ns.size() > 1) {
            double sum_sq_diff = 0.0;
            for (long long time_ns : deserialization_times_ns) {
                sum_sq_diff += std::pow(time_ns - average_duration_ns, 2);
            }
            double variance = sum_sq_diff / deserialization_times_ns.size();
            double std_dev_ns = std::sqrt(variance);
            std::cout << "标准差: " << std_dev_ns << " 纳秒" << std::endl;
        }
    } else {
        std::cout << "没有接收到任何消息进行反序列化测试。" << std::endl;
    }

    return 0;
}